#include "collisionDetection.h"
#include "gameMath.h"

//checks whether a sphere collides with a given triangle
//returns the offset needed the move the sphere off the plane
Vector sphereTriangleCollision(Vector triangleVertices[], Vector sphereCenter, float sphereRadius)
{
	//calculate the normal vector of the triangle
	Vector normalVector = triangleNormal(triangleVertices);

	//get the origin distance of the triangle
	float originDistance = planeOriginDistance(normalVector, triangleVertices[0]);

	//using the plane's equation (Ax + By + Cz + D) calculate distance from the plane
	float distance = (normalVector[0] * sphereCenter[0] + normalVector[1] * sphereCenter[1] + normalVector[2] * sphereCenter[2] + originDistance);

	//check collision with just the outer part of an object
	if (distance > 0)
		return Vector(0, 0, 0);

	//get the sphere position against the traignle
	int classification = spherePosition(sphereRadius, distance);

	if(classification == INTERSECTS)
	{
		Vector offset = normalVector * distance;
		Vector spherePositionProjectedOntoTrianglePlane = sphereCenter - offset;

		Vector closestPointOnLine(0, 0, 0);
		if(insideTriangle(spherePositionProjectedOntoTrianglePlane, triangleVertices))
			return getCollisionOffset(normalVector, sphereRadius, distance);
	}

	return Vector(0, 0, 0);
}

//checks whether player collides with the triangle edges
Vector sphereTriangleEdgeCollision(Vector triangleVertices[], Vector sphereCenter, float sphereRadius)
{
	Vector normalVector = triangleNormal(triangleVertices);
	float originDistance = planeOriginDistance(normalVector, triangleVertices[0]);
	float distance = (normalVector[0] * sphereCenter[0] + normalVector[1] * sphereCenter[1] + normalVector[2] * sphereCenter[2] + originDistance);
	if (distance > 0)
		return Vector(0, 0, 0);
	int classification = spherePosition(sphereRadius, distance);

	if(classification == INTERSECTS)
	{
		Vector closestPointOnLine(0, 0, 0);
		return getEdgeCollisionOffset(sphereCenter, triangleVertices, sphereRadius, distance);
	}

	return Vector(0, 0, 0);
}

//returns the position of the sphere against a given plane
//(whether it's in front of it, behind it or intersects it)
int spherePosition(float radius, float distance)
{
	if(absoluteValue(distance) < radius)
		return INTERSECTS;
	
	if(distance >= radius)
		return IS_IN_FRONT;
	
	//if it doesn't intersect it and it's not in front of it then it must be behid
	return IS_BEHIND;
}

//returns the offset to move the sphere off the colided edge of a triangle
Vector getEdgeCollisionOffset(Vector sphereCenter, Vector triangleVertices[], float sphereRadius, float distanceFromPlane)
{
	Vector pointOnLine(0, 0, 0);
	Vector vec(0, 0, 0);

	for(int i = 0; i < 3; i++)
	{
		//choose the closest point on every edge
		pointOnLine = closestPointOnLine(triangleVertices[i], triangleVertices[(i + 1) % 3], sphereCenter);
		float distance = distanceBetweenPoints(sphereCenter, pointOnLine);

		if (distance < sphereRadius)
		{
			vec = normalize(sphereCenter - pointOnLine);
			float distanceOver = sphereRadius - distance;
			vec *= distanceOver;
			return vec;
		}

	}

	return Vector(0, 0, 0);
}

//returns a point on line that is closest to the point
//passed as the third parameter
Vector closestPointOnLine(Vector pointA, Vector pointB, Vector point)
{
	//create two vectors - one from pointA to point
	//and the second from pointA to pointB (the second one is also normalized)
	Vector vector1 = point - pointA;
    Vector vector2 = normalize(pointB - pointA);

	//calculate the distance between two points on the line segment
    float d = distanceBetweenPoints(pointA, pointB);
    float t = dotProduct(vector2, vector1);

	//if the dot product is less or equal to 0
	//then the point must be the closest to pointA
    if (t <= 0) 
		return pointA;

	//if it's bigger than or equal to the distance between points on the line segment
	//pointB is the closest to point
    if (t >= d) 
		return pointB;
 
	//otherwise create another vector that is the length of t
	//and direction of vector2
    Vector vector3 = vector2 * t;

	//add it to pointA and you'll get the closest point
    return (pointA + vector3);
}

//checks whether a given line crosses the triangle
bool lineIntersectsPlane(Vector triangle[], Vector line[])
{
	float distance1 = 0, distance2 = 0;
			
	Vector normalVector = triangleNormal(triangle);

	float originDistance = planeOriginDistance(normalVector, triangle[0]);

	distance1 = ((normalVector[0] * line[0][0])  +
		         (normalVector[1] * line[0][1])  +
				 (normalVector[2] * line[0][2])) + originDistance;

	distance2 = ((normalVector[0] * line[1][0])  +
		         (normalVector[1] * line[1][1])  +
				 (normalVector[2] * line[1][2])) + originDistance;


	//if distance1 and distance2 have different signs
	//it means that they are on the opposite sides of the triangle
	//so that they collide with it
	if(distance1 * distance2 >= 0)
	   return false;
					
	return true;
}

//returns the offset to move the sphere off the colided triangle
Vector getCollisionOffset(Vector &normalVector, float sphereRadius, float distanceFromPlane)
{
	Vector offset = Vector(0, 0, 0);

	float distanceOver = sphereRadius + distanceFromPlane;
	offset = normalVector * -distanceOver;

	return offset;
}